#include <AccelStepper.h>
#include <WiFi.h>
#include <PubSubClient.h>

// ================== PIN DEFINITIONS ==================
#define STEP_PIN D1
#define DIR_PIN  D0
#define EN_PIN   D3

#define M0_PIN   D2
#define M1_PIN   D4
#define M2_PIN   D5

#define HALL_PIN D8

// ================= WIFI =================
const char* ssid = "Ali";
const char* password = "00000000";

// ================= MQTT =================
const char* mqtt_server = "mqtt.fabcloud.org";
const int mqtt_port = 1883;
const char* mqtt_user = "fabacademy";
const char* mqtt_pass = "fabacademy";

const char* sub_topic = "fabacademy/ashtami/tool";

WiFiClient espClient;
PubSubClient client(espClient);

// ================== HALL THRESHOLDS ==================
#define ON_THRESHOLD  3800
#define OFF_THRESHOLD 3400

// ================== TURNTABLE CONSTANTS ==================
#define MOTOR_STEPS   200
#define MICROSTEPS    16
#define GEAR_RATIO    5
#define STEPS_PER_REV (MOTOR_STEPS * MICROSTEPS * GEAR_RATIO)  // 16000

// ================== SPEED SETTINGS ==================
#define SEARCH_SPEED    800
#define BACKOFF_SPEED   400
#define APPROACH_SPEED  200

#define MOVE_MAX_SPEED  1200
#define MOVE_ACCEL      800
#define CRAWL_SPEED     300
#define CRAWL_THRESHOLD 200

// ================== LOOKUP TABLE ==================
// Formula: LUT[C][H] = (C_offset + H_offset) % 16000
//
// C offsets — 6 compartments equally spaced:
//   Nachos=0, Juice=2667, Lays=5333, Snickers=8000, Popcorn=10667, Cookies=13333
//
// H offsets — 5 positions equally spaced:
//   Abhishek=0, Ardra=3200, Ali=6400, Ashtami=9600, Mishael=12800
//
//                 [0]  [H1=Ab]  [H2=Ar]  [H3=Al]  [H4=As]  [H5=Mi]
const long LUT[7][6] = {
  {0,      0,      0,      0,      0,      0 }, // row 0 unused
  {0,      0,   3200,   6400,   9600,  12800 }, // C1 Nachos
  {0,   2667,   5867,   9067,  12267,  15467 }, // C2 Juice
  {0,   5333,   8533,  11733,  14933,   2133 }, // C3 Lays
  {0,   8000,  11200,  14400,   1600,   4800 }, // C4 Snickers
  {0,  10667,  13867,   1067,   4267,   7467 }, // C5 Popcorn
  {0,  13333,    533,   3733,   6933,  10133 }, // C6 Cookies
};

const char* COMP_LABEL[7] = {"", "Nachos","Juice","Lays","Snickers","Popcorn","Cookies"};
const char* HOME_LABEL[6] = {"", "Abhishek","Ardra","Ali","Ashtami","Mishael"};

// ================== STEPPER ==================
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// ================== HALL FILTER ==================
float filtered  = 0.0f;
bool  hallState = false;

// ================== STATE MACHINES ==================
enum HomingState { SEARCH, BACKOFF, APPROACH, HOMING_DONE };
enum SystemState { HOMING, POSITIONING, IDLE };

HomingState homingState = SEARCH;
SystemState sysState    = HOMING;

// ================== POSITION TRACKING ==================
long turntableStepPos = 0;
int  lastComp = 0;
int  lastHome = 0;

// ================== HALL SENSOR ==================
bool readHallDigital() {
  int raw = analogRead(HALL_PIN);
  filtered = 0.8f * filtered + 0.2f * raw;
  if (!hallState && filtered > ON_THRESHOLD) hallState = true;
  if ( hallState && filtered < OFF_THRESHOLD) hallState = false;
  return hallState;
}

// ================== HOMING ==================
void runHoming() {
  static bool lastHall = false;
  bool currentHall = readHallDigital();

  switch (homingState) {

    case SEARCH:
      stepper.setSpeed(SEARCH_SPEED);
      stepper.runSpeed();
      if (currentHall && !lastHall) {
        Serial.println("[HOMING] Magnet detected → BACKOFF");
        homingState = BACKOFF;
      }
      break;

    case BACKOFF:
      stepper.setSpeed(-BACKOFF_SPEED);
      stepper.runSpeed();
      if (!currentHall && lastHall) {
        Serial.println("[HOMING] Magnet cleared → APPROACH");
        homingState = APPROACH;
      }
      break;

    case APPROACH:
      stepper.setSpeed(APPROACH_SPEED);
      stepper.runSpeed();
      if (currentHall && !lastHall) {
        stepper.setSpeed(0);
        stepper.setCurrentPosition(0);
        turntableStepPos = 0;
        lastComp = 1;
        lastHome = 1;

        stepper.setMaxSpeed(MOVE_MAX_SPEED);
        stepper.setAcceleration(MOVE_ACCEL);

        homingState = HOMING_DONE;
        sysState    = IDLE;

        Serial.println("HOMING COMPLETE → Nachos at Abhishek");
      }
      break;

    case HOMING_DONE:
      break;
  }

  lastHall = currentHall;
}

// ================== MOVE (SHORTEST PATH) ==================
void moveCompartmentToHome(int c, int h) {

  if (c < 1 || c > 6) { Serial.println("[ERR] C must be 1-6"); return; }
  if (h < 1 || h > 5) { Serial.println("[ERR] H must be 1-5"); return; }

  long target = LUT[c][h];
  long delta  = target - turntableStepPos;

  // Shortest path
  if (delta >  STEPS_PER_REV / 2) delta -= STEPS_PER_REV;
  if (delta < -STEPS_PER_REV / 2) delta += STEPS_PER_REV;

  if (delta == 0) {
    Serial.println("[INFO] Already at target");
    return;
  }

  stepper.setMaxSpeed(MOVE_MAX_SPEED);
  stepper.moveTo(stepper.currentPosition() + delta);
  sysState = POSITIONING;

  turntableStepPos = target;
  lastComp = c;
  lastHome = h;

  Serial.print("Moving ");
  Serial.print(COMP_LABEL[c]);
  Serial.print(" → ");
  Serial.println(HOME_LABEL[h]);
  Serial.print("Steps: ");
  Serial.println(delta);
  Serial.print("Direction: ");
  Serial.println(delta > 0 ? "CW" : "CCW");
}

// ================== COMMAND PARSER ==================
void parseCommand(String cmd) {

  cmd.trim();
  cmd.toUpperCase();

  if (cmd.length() == 4 && cmd[0]=='C' && cmd[2]=='H') {
    int c = cmd[1] - '0';
    int h = cmd[3] - '0';
    moveCompartmentToHome(c, h);
    return;
  }

  if (cmd == "HOME") {
    homingState = SEARCH;
    sysState    = HOMING;
    stepper.setMaxSpeed(SEARCH_SPEED);
    Serial.println("[HOMING] Restarting...");
    return;
  }

  if (cmd == "POS") {
    Serial.print("Steps: ");
    Serial.println(turntableStepPos);
    return;
  }

  Serial.println("[ERR] Unknown command");
}

// ================== MQTT CALLBACK ==================
void callback(char* topic, byte* payload, unsigned int length) {

  Serial.println("\n MQTT MESSAGE RECEIVED");

  String msg = "";
  for (int i = 0; i < length; i++) {
    msg += (char)payload[i];
  }

  Serial.print("MQTT RAW: ");
  Serial.println(msg);

  msg.replace(":", "");

  Serial.print("Cleaned: ");
  Serial.println(msg);

  parseCommand(msg);
}

// ================== WIFI ==================
void setup_wifi() {
  Serial.print("Connecting WiFi");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\n WiFi Connected");
}

// ================== MQTT RECONNECT ==================
void reconnect() {
  while (!client.connected()) {
    Serial.println("Connecting MQTT...");
    if (client.connect("ESP32S3", mqtt_user, mqtt_pass)) {
      Serial.println("MQTT Connected");
      client.subscribe(sub_topic);
      Serial.print("Subscribed: ");
      Serial.println(sub_topic);
    } else {
      Serial.print("Failed rc=");
      Serial.println(client.state());
      delay(2000);
    }
  }
}

// ================== SETUP ==================
void setup() {

  Serial.begin(115200);

  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);

  pinMode(M0_PIN, OUTPUT); digitalWrite(M0_PIN, LOW);
  pinMode(M1_PIN, OUTPUT); digitalWrite(M1_PIN, LOW);
  pinMode(M2_PIN, OUTPUT); digitalWrite(M2_PIN, HIGH);

  filtered = analogRead(HALL_PIN);
  stepper.setMaxSpeed(SEARCH_SPEED);

  Serial.println("Starting homing...");

  setup_wifi();

  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(callback);

  Serial.println("MQTT Ready");
}

// ================== LOOP ==================
void loop() {

  if (!client.connected()) reconnect();
  client.loop();

  switch (sysState) {

    case HOMING:
      runHoming();
      break;

    case POSITIONING:
      if (stepper.distanceToGo() != 0) {

        if (abs(stepper.distanceToGo()) < CRAWL_THRESHOLD) {
          stepper.setMaxSpeed(CRAWL_SPEED);
        } else {
          stepper.setMaxSpeed(MOVE_MAX_SPEED);
        }

        stepper.run();

      } else {
        stepper.setMaxSpeed(MOVE_MAX_SPEED);
        Serial.println("Move complete");
        sysState = IDLE;
      }
      break;

    case IDLE:
      if (Serial.available()) {
        String cmd = Serial.readStringUntil('\n');
        parseCommand(cmd);
      }
      break;
  }
}